#ifndef __QUATERNION__
#define __QUATERNION__

#include <iostream>
using namespace std;

#include "processor.h"
#include "Triple.h"
#include "Matrix.h"

#define S 0
#define VX 1
#define VY 2
#define VZ 3

class __declspec( align( 16 ) ) Quaternion
{
	float coord[4];
	void *base_ptr;
public:
	float& operator[](const int index)
	{
	    return coord[index];
	}
	void* operator new(size_t);
	void Set(float s, float vx, float vy, float vz) { coord[S] = s; coord[VX] = vx; coord[VY] = vy; coord[VZ] = vz; }
	void print_ht(ostream& os, char *title);
	void Normalize();
	void Multiply(Quaternion& a, Quaternion& b);
	friend Quaternion operator*(Triple& omega, Quaternion& q); // dq = [0, omega] * q
	void operator*=(const float s);
	void operator=(Matrix& m);
	void Square(Quaternion *q);
	void Integrate(Quaternion& derivative, const FP_DATATYPE dt);
};

//inline void divide_quaternions(quaternion dest, const float scalar)
//{
//#ifdef SSSE3
//	sp128 a;
//	set(a, scalar, scalar, scalar, scalar);
//	void* aptr = &a;
//	__asm
//	{
//		mov eax,dest
//		mov edx,aptr
//		movaps xmm0,xmmword ptr [eax]
//		divps xmm0,xmmword ptr [edx]
//		movaps xmmword ptr [eax],xmm0
//	}
//#else
//	dest[0] /= scalar;
//	dest[1] /= scalar;
//	dest[2] /= scalar;
//	dest[3] /= scalar;
//#endif
//}

#ifdef SSSE3
void normalize_4q(float *q);
#endif

#endif
